import rospy
from std_msgs.msg import Int64,Float64
import sys
# sys.path.insert(0, "/home/tdb1/build_dir/tf_ws/devel/lib/python3/dist-packages/")
sys.path.insert(0, "/home/eaibot/anaconda3/envs/pac_2021/lib/python3.6/site-packages")
import rospy
from geometry_msgs.msg import Twist, Quaternion, Point
from laser_line_extraction.msg import LineSegment, LineSegmentList
from nav_msgs.msg import Odometry

import time
import numpy as np

from math import radians, copysign
from tf.transformations import euler_from_quaternion, quaternion_from_euler
from math import pi, sqrt, pow
import math

import tf2_ros

def quat_to_angle(quat):
    rot = euler_from_quaternion([quat.x, quat.y, quat.z, quat.w])
    # rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)
    return rot[2]


def normalize_angle(angle):
    res = angle
    while res > pi:
        res -= 2.0 * pi
    while res < -pi:
        res += 2.0 * pi
    return res


def get_foot(start, end):
    '''
    计算线段和机器人中心点作垂线的垂足坐标
    机器人中心点为[0,0]
    '''
    if start == end:
        return start
    start = np.array(start, dtype=float)
    end = np.array(end, dtype=float)
    ap = -start
    ab = end - start
    result = start + np.dot(ap, ab)/np.dot(ab, ab)*ab

    return result


def get_distance(point):

    return sqrt(pow((point[0]-0.30), 2) +
                pow((point[1]), 2))


class Slide():
    def __init__(self):
        # Subscribe line detection result
        rospy.init_node("visualizer_data")
        self.data_sub = rospy.Subscriber(
            '/cmd_vel', Twist, self.data_call, queue_size=2)
        self.data_pub = rospy.Publisher(
            '/linear_velocity', Float64, queue_size=2)
        # self.tf_buffer = tf2_ros.Buffer()
        # self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)

    def data_call(self, twist: Twist):
        # velocity = sqrt(pow((twist.linear.x), 2) +
        # trans = self.tf_buffer.lookup_transform(
        #         "map", "base_footprint", rospy.Time(0))
        print("x:{},y:{}".format(twist.linear.x,twist.linear.y))
        # print("tf x:{} y:{}".format(trans.transform.translation.x, trans.transform.translation.y))
        self.data_pub.publish(twist.linear.x)
        return
if __name__ == '__main__':

    slide = Slide()
    # time.sleep(2)
    # cal.slid = True
    # slide.target_position = slide.end_position
    # cal.slid = False
    while not rospy.is_shutdown():
        rospy.spin()
    # finally:
    #     rospy.on_shutdown()
    #     slide.line_sub.unregister()

    #     slide.shutdown()
